#include "self_localization_transform/transformer.h"





Transformer::Transformer(ros::NodeHandle& nh_){
    nh = nh_;

    sub_usv_sensor        = nh.subscribe<std_msgs::Float64MultiArray>("/control_state_0_1",10000,&Transformer::msg_callback,this);
    pub_transformed_data  = nh.advertise<self_localization_transform::usv_self>("usv_self",10000);

}



void Transformer::msg_callback(const std_msgs::Float64MultiArrayConstPtr& msg_in){


    CHECK(msg_in->data.size() == 17) << "msg_in data size != 17,Wrong msg.process killed.";
    


    self_localization_transform::usv_self msg_out;
    msg_out.header.frame_id = "usv";
    msg_out.header.stamp    = ros::Time::now();

    msg_out.v_enu.v_e       = msg_in->data[11];
    msg_out.v_enu.v_n       = msg_in->data[12];
    msg_out.v_enu.v_u       = msg_in->data[13];

    msg_out.gps.altitude    = msg_in->data[8];
    msg_out.gps.longitude   = msg_in->data[9];
    msg_out.gps.latitude    = msg_in->data[10];


    msg_out.imu.angular_velocity.x    = msg_in->data[5];
    msg_out.imu.angular_velocity.y    = msg_in->data[6];
    msg_out.imu.angular_velocity.z    = msg_in->data[7];
    msg_out.imu.linear_acceleration.x = msg_in->data[14];
    msg_out.imu.linear_acceleration.x = msg_in->data[15];
    msg_out.imu.linear_acceleration.x = msg_in->data[16]; 

    //TODO roll pithc yaw (deg) 转换成四元数
    
    Eigen::Quaternionf q_in = Eigen::AngleAxisf(msg_in->data[4],Eigen::Vector3f::UnitZ()) *
                              Eigen::AngleAxisf(msg_in->data[3],Eigen::Vector3f::UnitY()) * 
                              Eigen::AngleAxisf(msg_in->data[2],Eigen::Vector3f::UnitX()); 

    msg_out.imu.orientation.w = q_in.w();
    msg_out.imu.orientation.x = q_in.x();
    msg_out.imu.orientation.y = q_in.y();
    msg_out.imu.orientation.z = q_in.z();

    msg_out.ori_rpy.roll  = msg_in->data[2];
    msg_out.ori_rpy.pitch = msg_in->data[3];
    msg_out.ori_rpy.yaw   = msg_in->data[4];

    pub_transformed_data.publish(msg_out);

}